#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"

int main(int argc, char * argv[])
{
    // 初始化 ROS 2 系统
    rclcpp::init(argc, argv);

    // 创建 ROS 2 节点
    rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("tf_listener_node");

    // 创建时钟对象（必须传入 tf2_ros::Buffer 构造函数）
    rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);

    // 创建 tf2 缓冲区和监听器，传入节点和时钟对象
    tf2_ros::Buffer tf_buffer(clock);
    tf2_ros::TransformListener tf_listener(tf_buffer);

    // 设置定时器来定期监听变换
    rclcpp::WallRate loop_rate(1);  // 每秒 1 次

    while (rclcpp::ok())
    {
        try
        {
            // 尝试获取 "robot" 相对于 "world" 坐标系的变换
            geometry_msgs::msg::TransformStamped transformStamped = tf_buffer.lookupTransform("world", "robot", rclcpp::Time(0));

            // 打印接收到的变换信息
            RCLCPP_INFO(node->get_logger(), "Transform received: \nTranslation: (%.2f, %.2f, %.2f) \nRotation: (%.2f, %.2f, %.2f, %.2f)",
                        transformStamped.transform.translation.x,
                        transformStamped.transform.translation.y,
                        transformStamped.transform.translation.z,
                        transformStamped.transform.rotation.x,
                        transformStamped.transform.rotation.y,
                        transformStamped.transform.rotation.z,
                        transformStamped.transform.rotation.w);
        }
        catch (tf2::TransformException & ex)
        {
            RCLCPP_WARN(node->get_logger(), "Could not transform: %s", ex.what());
        }

        rclcpp::spin_some(node);  // 处理回调
        loop_rate.sleep();  // 等待下一次循环
    }

    rclcpp::shutdown();
    return 0;
}
